﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Microsoft.Xna.Framework;
using Microsoft.Xna.Framework.Input;
using Brain.Bepu;
using BEPUphysics;

namespace Brain.FPS
{
    public class DefaultGamerControllerInput : IGamerControllerInput
    {
        /// <summary>
        /// If this Input Manager should Update
        /// </summary>
        public bool ShouldUpdate { get; set; }

        /// <summary>
        /// The Gamer Controller parent of this Input Manager
        /// </summary>
        public IGamerController Controller { get; set; }

        /// <summary>
        /// Physics representation of the character.
        /// </summary>
        public CharacterController CharacterController;

        /// <summary>
        /// Owning space of the character.
        /// </summary>
        public Space Space { get; private set; }

        public GamerInputOptions Options { get; set; }

        public DefaultGamerControllerInput(IGamerController gamer, float height, float radius)
        {
            Options = new GamerInputOptions();

            CharacterController = new CharacterController(height, radius);

            this.Space = (Engine.Instance.Physics3DManager as BepuPhysicsManager).Space;
            Space.Add(CharacterController);

            this.Controller = gamer;
            ShouldUpdate = true;
        }

        public void Update()
        {
            float dt = (float)Engine.Instance.GameTime.ElapsedGameTime.TotalSeconds;

            IGamerCamera Camera = this.Controller.Camera;
            PlayerIndex Index = this.Controller.PlayerInfo.PlayerIndex;

            if (Options.CanCrouch)
            {
                this.Controller.IsCrouching = CharacterController.StanceManager.CurrentStance == Stance.Standing;
            }
            else
            {
                this.Controller.IsCrouching = false;
            }
            CharacterInfo character = this.Controller.CharInfo;

            //Note that the character controller's update method is not called here; this is because it is handled within its owning space.
            //This method's job is simply to tell the character to move around based on the Camera and input.
            if (Options.UseCameraSmoothing)
            {
                //First, find where the camera is expected to be based on the last position and the current velocity.
                //Note: if the character were a free-floating 6DOF character, this would need to include an angular velocity contribution.
                //And of course, the camera orientation would be based on the character's orientation.
                Camera.Position = Camera.Position + CharacterController.Body.LinearVelocity * dt;
                //Now compute where it should be according the physical body of the character.
                Vector3 up = CharacterController.Body.OrientationMatrix.Up;
                Vector3 bodyPosition = CharacterController.Body.Position;
                Vector3 goalPosition = bodyPosition + up * (this.Controller.IsCrouching ? character.StandingCameraOffset : character.CrouchingCameraOffset);

                //Usually, the camera position and the goal will be very close, if not matching completely.
                //However, if the character steps or has its position otherwise modified, then they will not match.
                //In this case, we need to correct the camera position.

                //To do this, first note that we can't correct infinite errors.  We need to define a bounding region that is relative to the character
                //in which the camera can interpolate around.  The most common discontinuous motions are those of upstepping and downstepping.
                //In downstepping, the character can teleport up to the character's MaximumStepHeight downwards.
                //In upstepping, the character can teleport up to the character's MaximumStepHeight upwards, and the body's CollisionMargin horizontally.
                //Picking those as bounds creates a constraining cylinder.

                Vector3 error = goalPosition - Camera.Position;
                float verticalError = Vector3.Dot(error, up);
                Vector3 horizontalError = error - verticalError * up;
                //Clamp the vertical component of the camera position within the bounding cylinder.
                if (verticalError > CharacterController.StepManager.MaximumStepHeight)
                {
                    Camera.Position -= up * (CharacterController.StepManager.MaximumStepHeight - verticalError);
                    verticalError = CharacterController.StepManager.MaximumStepHeight;
                }
                else if (verticalError < -CharacterController.StepManager.MaximumStepHeight)
                {
                    Camera.Position -= up * (-CharacterController.StepManager.MaximumStepHeight - verticalError);
                    verticalError = -CharacterController.StepManager.MaximumStepHeight;
                }
                //Clamp the horizontal distance too.
                float horizontalErrorLength = horizontalError.LengthSquared();
                float margin = CharacterController.Body.CollisionInformation.Shape.CollisionMargin;
                if (horizontalErrorLength > margin * margin)
                {
                    Vector3 previousHorizontalError = horizontalError;
                    Vector3.Multiply(ref horizontalError, margin / (float)Math.Sqrt(horizontalErrorLength), out horizontalError);
                    Camera.Position -= horizontalError - previousHorizontalError;
                    horizontalErrorLength = margin * margin;
                }
                //Now that the error/camera position is known to lie within the constraining cylinder, we can perform a smooth correction.

                //This removes a portion of the error each frame.
                //Note that this is not framerate independent.  If fixed time step is not enabled,
                //a different smoothing method should be applied to the final error values.
                //float errorCorrectionFactor = .3f;

                //This version is framerate independent, although it is more expensive.
                float errorCorrectionFactor = (float)(1 - Math.Pow(.000000001, dt));
                Camera.Position += up * (verticalError * errorCorrectionFactor);
                Camera.Position += horizontalError * errorCorrectionFactor;
            }
            else
            {
                Camera.Position = CharacterController.Body.Position + (this.Controller.IsCrouching ? character.StandingCameraOffset : character.CrouchingCameraOffset) * CharacterController.Body.OrientationMatrix.Up;
            }

            Vector2 totalMovement = Vector2.Zero;

            Vector3 forward = Camera.RotationMatrix.Forward;
            forward.Y = 0;
            forward.Normalize();
            Vector3 right = Camera.RotationMatrix.Right;

            Vector2 leftThumbstick = Engine.Instance.InputManager.GetLeftThumbstick(this.Controller.PlayerInfo.PlayerIndex);
            this.LeftThumbstick = leftThumbstick;

            if (Options.DisableLeftRight)
            {
                leftThumbstick.X = 0;
            }
            if (Options.DisableUpDown)
            {
                leftThumbstick.Y = 0;
            }

            if (Options.MovementCondition == null || Options.MovementCondition(leftThumbstick))
            {
                totalMovement += leftThumbstick.Y * new Vector2(forward.X, forward.Z);
                totalMovement += leftThumbstick.X * new Vector2(right.X, right.Z);
                CharacterController.HorizontalMotionConstraint.MovementDirection = Vector2.Normalize(totalMovement);
            }
            else
            {
                CharacterController.HorizontalMotionConstraint.MovementDirection = Vector2.Zero;
                CharacterController.Body.LinearVelocity = Vector3.Zero;
            }

            if (Options.CanCrouch)
            {
                if (Options.CrouchHold)
                {
                    bool btn = Engine.Instance.InputManager.IsButtonDown(Buttons.RightStick, Index);
                    bool key = Engine.Instance.InputManager.IsKeyDown(Keys.Z);
                    if (btn || key)
                    {
                        CharacterController.StanceManager.DesiredStance = Stance.Standing;
                    }
                    else
                    {
                        CharacterController.StanceManager.DesiredStance = Stance.Crouching;
                    }
                }
                else
                {
                    if (this.Controller.IsCrouching)
                    {
                        CharacterController.StanceManager.DesiredStance = Stance.Standing;
                    }
                    else
                    {
                        CharacterController.StanceManager.DesiredStance = Stance.Crouching;
                    }
                }
            }

            //Jumping
            if (Options.CanJump)
            {
                if (Engine.Instance.InputManager.IsButtonPress(Buttons.A, Index) ||
                    Engine.Instance.InputManager.IsKeyPress(Keys.Space))
                {
                    CharacterController.Jump();
                }
            }

            //Collect the movement impulses.

            Vector3 movementDir;

            if (!Options.DisableUpDown)
            {
                if (Engine.Instance.InputManager.IsKeyDown(Keys.W))
                {
                    movementDir = Camera.RotationMatrix.Forward;
                    totalMovement += Vector2.Normalize(new Vector2(movementDir.X, movementDir.Z));
                }
                if (Engine.Instance.InputManager.IsKeyDown(Keys.S))
                {
                    movementDir = Camera.RotationMatrix.Forward;
                    totalMovement -= Vector2.Normalize(new Vector2(movementDir.X, movementDir.Z));
                }
            }
            if (!Options.DisableLeftRight)
            {
                if (Engine.Instance.InputManager.IsKeyDown(Keys.A))
                {
                    movementDir = Camera.RotationMatrix.Left;
                    totalMovement += Vector2.Normalize(new Vector2(movementDir.X, movementDir.Z));
                }
                if (Engine.Instance.InputManager.IsKeyDown(Keys.D))
                {
                    movementDir = Camera.RotationMatrix.Right;
                    totalMovement += Vector2.Normalize(new Vector2(movementDir.X, movementDir.Z));
                }
            }
            if (totalMovement == Vector2.Zero)
            {
                this.Controller.IsRunning = false;
                this.Controller.IsWalking = false;
                CharacterController.HorizontalMotionConstraint.MovementDirection = Vector2.Zero;
            }
            else
            {
                if (Options.MovementCondition == null || Options.MovementCondition(totalMovement))
                {
                    this.Controller.IsRunning = false;
                    this.Controller.IsWalking = true;
                    CharacterController.HorizontalMotionConstraint.MovementDirection = Vector2.Normalize(totalMovement);
                }
                else
                {
                    this.Controller.IsRunning = false;
                    this.Controller.IsWalking = false;
                    CharacterController.HorizontalMotionConstraint.MovementDirection = Vector2.Zero;
                    CharacterController.Body.LinearVelocity = Vector3.Zero;
                }
            }


            // Camera Rotation
            Vector2 rightThumb = Engine.Instance.InputManager.GetRightThumbstick(Controller.PlayerInfo.PlayerIndex);
            this.RightThumbstick = rightThumb;
            if (!Options.RightThumstickRotates)
            {
                rightThumb = Vector2.Zero;
            }

            if (Options.MouseRotates)
            {
                Vector2 mouseDif = Engine.Instance.InputManager.GetMouseDifference();
                rightThumb += new Vector2(mouseDif.X, -mouseDif.Y);
            }

            if (Options.MouseRotates || Options.RightThumstickRotates)
            {
                Vector3 rot = Camera.RotationRadians + Vector3Util.ToRadians(new Vector3(rightThumb.Y, -rightThumb.X, 0));
                float seventy = MathHelper.ToRadians(70);
                Controller.Camera.RotationRadians = new Vector3(FloatUtil.Clamp(rot.X, -seventy, seventy), rot.Y, 0);
            }

            this.Controller.IsScoping = false;
            if (!this.Controller.IsRunning)
            {
                if (Engine.Instance.InputManager.IsMouseRightDown() ||
                    Engine.Instance.InputManager.IsButtonDown(Buttons.LeftTrigger, Controller.PlayerInfo.PlayerIndex))
                {
                    this.Controller.IsScoping = true;
                }
                else
                {
                    this.Controller.IsScoping = false;
                }
            }

            if (this.Controller.IsWalking)
            {
                if (this.Controller.IsRunning)
                {
                    CharacterController.HorizontalMotionConstraint.Speed = character.RunSpeed;
                }
                else
                {
                    if (this.Controller.IsScoping)
                    {
                        CharacterController.HorizontalMotionConstraint.Speed = character.ScopeSpeed;
                    }
                    else
                    {
                        CharacterController.HorizontalMotionConstraint.Speed = character.WalkSpeed;
                    }
                }
            }

            if (Engine.Instance.InputManager.IsButtonDown(Buttons.RightTrigger, this.Controller.PlayerInfo.PlayerIndex) ||
                Engine.Instance.InputManager.IsMouseLeftDown())
            {
                Controller.Weapon.Shoot();
            }
        }


        public void SetPosition(Vector3 pos)
        {
            this.CharacterController.Body.Position = pos;
        }


        public Vector2 LeftThumbstick { get; set; }

        public Vector2 RightThumbstick { get; set; }
    }
}
